#include "ModelBoundingVolume.h"
#include "Cooking.h"
#include "../Stream.h"


/************************************************************************/
/*                        PUBLIC METHODS                                */
/************************************************************************/


ModelBoundingVolume::ModelBoundingVolume(NxPhysicsSDK *sdk)
{
    this->sdk = sdk;
    this->errLog = NULL;
}

ModelBoundingVolume::ModelBoundingVolume(NxPhysicsSDK *sdk, Log *log)
{
    this->sdk = sdk;
    this->errLog = log;
}

ModelBoundingVolume::~ModelBoundingVolume()
{
    if(this->shapeDescPointer != NULL)
    {
        delete shapeDescPointer;
    }
}

void ModelBoundingVolume::CalcBoundingVolume(SHAPETYPE shapetype, const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 complNodeTransf, float modelScaleFactor)
{
    this->modelShapeType = shapetype;

    if(this->modelShapeType == BOX_SHAPE)
    {
        bool success = CalcBoxShape(modelAiInfo, complNodeTransf, this->shapeDescPointer, modelScaleFactor);
        if(this->errLog != NULL)
        {
            if(success)
            {
                this->errLog->LogDebug(StringBuilder() << "Calculated BOX_SHAPE (" << modelAiInfo.node->mName.data << ").");
            }
            else
            {
                this->errLog->LogError("BOX_SHAPE couln't be calculated.");
            }			
        }
    }
    else if(this->modelShapeType == TRIANGLE_MESH_SHAPE)
    {
        bool success = CalcTriangleMeshShapes(modelAiInfo, complNodeTransf, this->shapeDescPointer, modelScaleFactor);
        if(this->errLog != NULL)
        {
            if(success)
            {
                this->errLog->LogDebug(StringBuilder() << "Calculated TRIANGLE_MESH_SHAPE (" << modelAiInfo.node->mName.data << ").");
            }
            else
            {
                this->errLog->LogError("TRIANGLE_MESH_SHAPE couln't be calculated.");
            }			
        }			
    }
    else if(this->modelShapeType == CONVEX_SHAPE)
    {
        bool success = CalcConvexShapes(modelAiInfo, complNodeTransf, this->shapeDescPointer, modelScaleFactor);
        if(this->errLog != NULL)
        {
            if(success)
            {
                this->errLog->LogDebug(StringBuilder() << "Calculated CONVEX_SHAPE (" << modelAiInfo.node->mName.data << ").");
            }
            else
            {
                this->errLog->LogError("CONVEX_SHAPE couln't be calculated.");
            }			
        }			
    }
}

NxShapeDesc *ModelBoundingVolume::GetShapeDesc()
{
    return this->shapeDescPointer;
}

NxBoxShapeDesc *ModelBoundingVolume::IsBoxShape()
{
    if(this->modelShapeType == BOX_SHAPE)
    {
        return (NxBoxShapeDesc*)(this->shapeDescPointer);
    }
    else
    {
        return NULL;
    }
}

NxTriangleMeshShapeDesc *ModelBoundingVolume::IsTriangleMeshShape()
{
    if(this->modelShapeType == TRIANGLE_MESH_SHAPE)
    {
        return (NxTriangleMeshShapeDesc*)(this->shapeDescPointer);
    }
    else
    {
        return NULL;
    }
}

NxConvexShapeDesc *ModelBoundingVolume::IsConvexShape()
{
    if(this->modelShapeType == CONVEX_SHAPE)
    {
        return (NxConvexShapeDesc*)(this->shapeDescPointer);
    }
    else
    {
        return NULL;
    }
}


/************************************************************************/
/*                        PRIVATE METHODS                               */
/************************************************************************/

bool ModelBoundingVolume::CalcBoxShape(const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 parentTransformation, NxShapeDesc *&shapeDescPointer, float scalefactor)
{

    //const aiScene *scene = modelAiInfo.scene;
    const aiNode *node = modelAiInfo.node;
    const aiMesh *mesh = modelAiInfo.mesh;

    //calc bounding box
    aiVector3D min(0.0f, 0.0f, 0.0f);
    aiVector3D max(0.0f, 0.0f, 0.0f);

    for(unsigned int i = 0; i < mesh->mNumVertices; ++i)
    {
        aiVector3D *vertex = (&mesh->mVertices[i]);

        if(i == 0)
        {
            min.x = vertex->x * scalefactor;
            min.y = vertex->y * scalefactor;
            min.z = vertex->z * scalefactor;

            max.x = vertex->x * scalefactor;
            max.y = vertex->y * scalefactor;
            max.z = vertex->z * scalefactor;
            continue;
        }

        if(vertex->x * scalefactor < min.x) min.x = vertex->x * scalefactor;
        if(vertex->y * scalefactor < min.y) min.y = vertex->y * scalefactor;
        if(vertex->z * scalefactor < min.z) min.z = vertex->z * scalefactor;

        if(vertex->x * scalefactor > max.x) max.x = vertex->x * scalefactor;
        if(vertex->y * scalefactor > max.y) max.y = vertex->y * scalefactor;
        if(vertex->z * scalefactor > max.z) max.z = vertex->z * scalefactor;
    }

    float xSize,ySize,zSize;
    xSize = (max.x - min.x) / 2.0f;
    ySize = (max.y - min.y) / 2.0f;
    zSize = (max.z - min.z) / 2.0f;

    NxVec3 centerOfBB(min.x + xSize, min.y + ySize,  min.z + zSize);
    NxBoxShapeDesc *boxShape = new NxBoxShapeDesc();
    boxShape->name = node->mName.data;
    boxShape->dimensions = NxVec3(xSize,ySize,zSize);

    NxMat33 matrix;
    matrix.setRow(0, NxVec3(parentTransformation.a1, parentTransformation.a2, parentTransformation.a3));
    matrix.setRow(1, NxVec3(parentTransformation.b1, parentTransformation.b2, parentTransformation.b3));
    matrix.setRow(2, NxVec3(parentTransformation.c1, parentTransformation.c2, parentTransformation.c3));
    
    boxShape->localPose.M = matrix;
    if(strcmp(modelAiInfo.modelfileExtension.c_str(), ".obj") == 0/*no hierarchial data*/)
    {
        NxVec3 newCenter(0.0f, 0.0f, 0.0f);
        matrix.multiply(centerOfBB,newCenter);
        boxShape->localPose.t = newCenter;
    }
    else
    {
        boxShape->localPose.t = NxVec3(parentTransformation.a4, parentTransformation.b4, parentTransformation.c4);
    }
    shapeDescPointer = boxShape;

    return true;
}

bool ModelBoundingVolume::CalcTriangleMeshShapes(const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 parentTransformation, NxShapeDesc *&shapeDescPointer, float scalefactor)
{
    //const aiScene *scene = modelAiInfo.scene;
    const aiNode *node = modelAiInfo.node;
    const aiMesh *mesh = modelAiInfo.mesh;

    NxVec3 *triangleMeshVertices = new NxVec3[mesh->mNumVertices];
    for (unsigned int t = 0; t < mesh->mNumVertices; ++t) {
        aiVector3D vertice = mesh->mVertices[t];
        triangleMeshVertices[t] = NxVec3(vertice.x * scalefactor, vertice.y * scalefactor, vertice.z * scalefactor);
    }

    NxU32 *triangleMeshFaces = new NxU32[mesh->mNumFaces*3];
    for (unsigned int t = 0; t < mesh->mNumFaces; ++t) {
        const struct aiFace* face = &mesh->mFaces[t];
        for(unsigned int i = 0; i < 3; i++) {
            int index = face->mIndices[i];
            triangleMeshFaces[3*t+i] = index;
        }
    }

    NxTriangleMeshDesc triangleMeshDesc;
    triangleMeshDesc.numVertices				= mesh->mNumVertices;
    triangleMeshDesc.numTriangles				= mesh->mNumFaces;
    triangleMeshDesc.pointStrideBytes			= sizeof(NxVec3);
    triangleMeshDesc.triangleStrideBytes		= 3*sizeof(NxU32);
    triangleMeshDesc.points						= triangleMeshVertices;
    triangleMeshDesc.triangles					= triangleMeshFaces;							
    triangleMeshDesc.flags						= 0;	

    Cooking::InitCooking();
    MemoryWriteBuffer buf;
    if(Cooking::CookTriangleMesh(triangleMeshDesc, buf))
    {
        MemoryReadBuffer readBuffer(buf.data);
        NxTriangleMeshShapeDesc *triangleMeshShape = new NxTriangleMeshShapeDesc();
        triangleMeshShape->meshData = sdk->createTriangleMesh(readBuffer);
        triangleMeshShape->name = node->mName.data;
        shapeDescPointer = triangleMeshShape;

        NxMat33 matrix;
        matrix.setRow(0, NxVec3(parentTransformation.a1, parentTransformation.a2, parentTransformation.a3));
        matrix.setRow(1, NxVec3(parentTransformation.b1, parentTransformation.b2, parentTransformation.b3));
        matrix.setRow(2, NxVec3(parentTransformation.c1, parentTransformation.c2, parentTransformation.c3));

        NxVec3 pos(parentTransformation.a4, parentTransformation.b4, parentTransformation.c4);
        shapeDescPointer->localPose.M = matrix;
        shapeDescPointer->localPose.t = pos;
    }	
    else
    {
        return false;
    }
    
    delete[] triangleMeshVertices;
    delete[] triangleMeshFaces;

    return true;
}

bool ModelBoundingVolume::CalcConvexShapes(const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 parentTransformation, NxShapeDesc *&shapeDescPointer, float scalefactor)
{
    //const aiScene *scene = modelAiInfo.scene;
    const aiNode *node = modelAiInfo.node;
    const aiMesh *mesh = modelAiInfo.mesh;

    NxVec3 *convexMeshVertices = new NxVec3[mesh->mNumVertices];
    for (unsigned int t = 0; t < mesh->mNumVertices; ++t) {
        aiVector3D vertice = mesh->mVertices[t];
        convexMeshVertices[t] = NxVec3(vertice.x * scalefactor, vertice.y * scalefactor, vertice.z * scalefactor);
    }

    NxU32 *convexMeshFaces = new NxU32[mesh->mNumFaces*3];
    for (unsigned int t = 0; t < mesh->mNumFaces; ++t) {
        const struct aiFace* face = &mesh->mFaces[t];
        for(unsigned int i = 0; i < 3; i++) {
            int index = face->mIndices[i];
            convexMeshFaces[3*t+i] = index;
        }
    }

    NxConvexMeshDesc convexMeshDesc;
    convexMeshDesc.numVertices				= mesh->mNumVertices;
    convexMeshDesc.numTriangles				= mesh->mNumFaces;
    convexMeshDesc.pointStrideBytes			= sizeof(NxVec3);
    convexMeshDesc.triangleStrideBytes		= 3*sizeof(NxU32);
    convexMeshDesc.points					= convexMeshVertices;
    convexMeshDesc.triangles				= convexMeshFaces;							
    convexMeshDesc.flags					= NX_CF_COMPUTE_CONVEX;

    Cooking::InitCooking();
    MemoryWriteBuffer buf;
    if(Cooking::CookConvexMesh(convexMeshDesc, buf))
    {
        MemoryReadBuffer readBuffer(buf.data);
        NxConvexShapeDesc *convexMeshShape = new NxConvexShapeDesc();
        convexMeshShape->meshData = sdk->createConvexMesh(readBuffer);

        NxMat33 matrix;
        matrix.setRow(0, NxVec3(parentTransformation.a1, parentTransformation.a2, parentTransformation.a3));
        matrix.setRow(1, NxVec3(parentTransformation.b1, parentTransformation.b2, parentTransformation.b3));
        matrix.setRow(2, NxVec3(parentTransformation.c1, parentTransformation.c2, parentTransformation.c3));

        NxVec3 pos(parentTransformation.a4, parentTransformation.b4, parentTransformation.c4);
        convexMeshShape->localPose.M = matrix;
        convexMeshShape->localPose.t = pos;
        convexMeshShape->name = node->mName.data;
        shapeDescPointer = convexMeshShape;
    }	
    else
    {
        return false;
    }

    delete[] convexMeshVertices;
    delete[] convexMeshFaces;

    return true;
}
